//
// Created by augtons on 2022/1/9.
//

#include "wind_swing_command.h"
#include "windswing_specific.h"

void reset();
void positive_rotation(int motoIndex);
void negative_rotation(int motoIndex);
void stop_rotation(int motoIndex);
void left_right_swing();
void angle_swing(int angle);
void shut_down();
void draw_circle(int radium);

void windswing_c_command(const char *rawCmd){ // without "C" at head
    switch (rawCmd[0]) {
        case 'S': { //CS reset
            reset();
            break;
        }
        case 'A': { //CAN2 CAP3 CAS4
            if (rawCmd[1] == 'P'){      //正转
                positive_rotation(rawCmd[2]-'0');
            }else if(rawCmd[1] == 'N'){ //反转
                negative_rotation(rawCmd[2]-'0');
            }else if(rawCmd[1] == 'S'){ //停止
                stop_rotation(rawCmd[2]-'0');
            }
            break;
        }
        case 'B':{  //CBO:左右自由摆动，20cm以上的直线段
            if(rawCmd[1] == 'O'){
                left_right_swing();
            }
            break;
        }
        case 'C': { //CC30 :15s指定角度(30)直线摆动[发挥]
            int angle = -1;
            if(1 == sscanf(rawCmd + 1, "%d", &angle)) {
                if (angle >= 0 && angle <= 179) {
                    angle_swing(angle);
                }
            }
            break;
        }
        case 'D': { //CD:10s 制动[发挥]
            shut_down();
            break;
        }
        case 'E': { //CE25: 30s 内画3次圆圈[发挥]
            int theta = -1;
            if(1 == sscanf(rawCmd + 1, "%d", &theta)){
                if(theta >= 0 && theta <= 60){
                    draw_circle(theta);
                }
            }
            break;
        }
    }
}

void windswing_d_command(const char *rawCmd){
    int len = -1;
    if(1 == sscanf(rawCmd, "%d", &len)){
        if(len > 0 && len <= 200){
            ESP_LOGI("SWING", "swing_len = %d", len);
            swing_len = len;
        }
    }
}

inline void reset(){
    windSwingBehavior = BEHAVIOR_RESET;
    ESP_LOGI("SWING", "reset");
    if(currentTask != NULL){
        vTaskDelete(currentTask);
        currentTask = NULL;
    }
    if(currentTask2 != NULL){
        vTaskDelete(currentTask2);
        currentTask2 = NULL;
    }
    behavior_data = 0;
    motoPwm.l = 0;
    motoPwm.r = 0;
    moto_stop(0);
    moto_stop(1);
    moto_stop(2);
    moto_stop(3);
}

void positive_rotation(int motoIndex){
    windSwingBehavior = BEHAVIOR_SINGLE_MOTO;
    ESP_LOGI("SWING", "positive_rotation = %d", motoIndex);
    moto_forward(motoIndex - 1, 20);
}

void negative_rotation(int motoIndex){
    windSwingBehavior = BEHAVIOR_SINGLE_MOTO;
    ESP_LOGI("SWING", "negative_rotation = %d", motoIndex);
    moto_backward(motoIndex - 1, 20);
}

void stop_rotation(int motoIndex){
    windSwingBehavior = BEHAVIOR_SINGLE_MOTO;
    ESP_LOGI("SWING", "stop_rotation = %d", motoIndex);
    moto_stop(motoIndex - 1);
}

void left_right_swing(){
    windSwingBehavior = BEHAVIOR_LEFT_RIGHT_SWING;
    ESP_LOGI("SWING", "left_right_swing");
}

void angle_swing(int angle){
    reset();
    windSwingBehavior = BEHAVIOR_ANGLE_SWING;
    behavior_data = angle;
    xTaskCreate(task_line, "task_line", BEHAVIOR_TASK_S, NULL, BEHAVIOR_TASK_P, &currentTask);
    ESP_LOGI("SWING", "angle_swing = %d", angle);
}

void shut_down(){
    reset();
    windSwingBehavior = BEHAVIOR_SHUTDOWN;
    ESP_LOGI("SWING", "shut_down");
    xTaskCreate(task_stop_self, "task_stop_self", BEHAVIOR_TASK_S, NULL, BEHAVIOR_TASK_P, &currentTask);
}

void draw_circle(int theta){
    reset();
    windSwingBehavior = BEHAVIOR_DRAW_CIRCLE;
    ESP_LOGI("SWING", "draw_circle = %d", theta);
    behavior_data = theta;
    xTaskCreate(task_circle, "task_circle", BEHAVIOR_TASK_S, NULL, BEHAVIOR_TASK_P, &currentTask);
}